/*
 * FM Transmitter Control
 *
 * Description: This code controls a NS73M via I2C using a 2 button interface
 *              with a 4-digit 7-segment display.
 *
 * File:   main.c
 * Author: Brennan L. Ujcich
 *
 * Created on May 26, 2013
 *
 * Last Modified on November 15, 2013
 */

#include <p18cxxx.h>
#include <p18f45k22.h>
#include <i2c.h>
#include <delays.h>
#include <math.h>

#pragma config FOSC = INTIO67, PLLCFG = OFF, PRICLKEN = ON, FCMEN = ON, PWRTEN = OFF
#pragma config BOREN = SBORDIS, BORV = 250, WDTEN = OFF, WDTPS = 4096, PBADEN = OFF
#pragma config HFOFST = OFF, MCLRE = EXTMCLR, STVREN = ON, LVP = OFF, DEBUG = ON

//Define memory locations
#define powerMem         0            // Power Mode
#define presetModeMem    1            // Preset Mode
#define presetNumMem     2            // Preset Number
#define frequencyMem     3            // Preset Number

//Define constants for FM
#define topFM  107900000            // Top of the FM Dial Range in USA
#define botFM   87500000            // Bottom of the FM Dial Range in USA
#define incrFM    200000            // FM Channel Increment in USA

//I2C variables
unsigned char Slave_address = 0b11001110; //Write address of NS73M FM Transmitter
signed char I2C_error = 0;

//Seven Segment Array
char sevenseg[4] = {0, 0, 0, 0};

//Miscellaeous Settings
int power = 0;
char presetNum = 0;
char presetMode = 0;
unsigned long userFreq = 105300000;

//Preset Constants
const int NUM_PRESETS = 3;
char presetName[3][4] = {
    {'c', 'b', 'u', 's'},
    {' ', 'c', 'd', 'r'},
    {'v', 'v', 'o', 'o'}
};
long presetFreq[3] = {92900000, 105300000, 94500000};

//Various Iterators
int i = 0;
int pressCount = 0;
int breakApartIterate = 0;
int fillIterate = 0;

/*
 *
 */
void low_ISR(void);
void high_ISR(void);
void seven_segment_display(void);
#pragma code high_vector = 0x08	// force the following statement to start at

void high_interrupt(void) // 0x08
{
    _asm
    goto high_ISR
            _endasm
}
#pragma code 			//return to the default code section
#pragma interrupt high_ISR

void high_ISR(void) {
    //Is this a rising or falling edge
    if (INTCONbits.TMR0IF == 1) {
        seven_segment_display();
    }
    INTCONbits.TMR0IF = 0;
}

//This function sets up the Seven Segment ports
void seven_segment_init() {
    LATB = 0x00;
    ANSELB = 0b11011111; //5 is for interrupt button
    TRISB = 0b11111111;

    PORTA = 0x00;
    ANSELA = 0xFF;
    TRISA = 0b00001111;

    T0CON = 0b11000010;

    RCONbits.IPEN = 1;
    INTCONbits.GIEH = 1;
    INTCONbits.GIEL = 0;
    INTCONbits.TMR0IE = 1;
    INTCON2bits.TMR0IP = 1;

}

//This function is called by the ISR and takes the sevenseg array and displays it
void seven_segment_display() {
    //Reset everything
    PORTA = 0x00; //Turn off the power to all segments
    TRISA = 0b00001111; //Reset digits
    TRISB = 0xFF; //Reset digits

    //Turn on power to appropriate digit
    if (i == 0) {
        PORTAbits.RA7 = 1;
        if (power == 1) {
            TRISAbits.TRISA1 = 0; //Decimal point
        }
    } else if (i == 1) {
        PORTAbits.RA6 = 1;
    } else if (i == 2) {
        PORTAbits.RA5 = 1;
        TRISAbits.TRISA1 = 0; //Decimal point
    } else if (i == 3) {
        PORTAbits.RA4 = 1;
    }

    //Light up digit
    switch (sevenseg[i]) {
        case '1':
            TRISBbits.TRISB2 = 0; //B
            TRISBbits.TRISB1 = 0; //C
            break;
        case '2':
            TRISBbits.TRISB4 = 0; //A
            TRISBbits.TRISB2 = 0; //B
            TRISAbits.TRISA0 = 0; //D
            TRISAbits.TRISA2 = 0; //E
            TRISBbits.TRISB3 = 0; //G
            break;
        case '3':
            TRISBbits.TRISB4 = 0; //A
            TRISBbits.TRISB2 = 0; //B
            TRISBbits.TRISB1 = 0; //C
            TRISAbits.TRISA0 = 0; //D
            TRISBbits.TRISB3 = 0; //G
            break;
        case '4':
            TRISBbits.TRISB2 = 0; //B
            TRISBbits.TRISB1 = 0; //C
            TRISBbits.TRISB0 = 0; //F
            TRISBbits.TRISB3 = 0; //G
            break;
        case '5':
            TRISBbits.TRISB4 = 0; //A
            TRISBbits.TRISB1 = 0; //C
            TRISAbits.TRISA0 = 0; //D
            TRISBbits.TRISB0 = 0; //F
            TRISBbits.TRISB3 = 0; //G
            break;
        case '6':
            TRISBbits.TRISB4 = 0; //A
            TRISBbits.TRISB1 = 0; //C
            TRISAbits.TRISA0 = 0; //D
            TRISAbits.TRISA2 = 0; //E
            TRISBbits.TRISB0 = 0; //F
            TRISBbits.TRISB3 = 0; //G
            break;
        case '7':
            TRISBbits.TRISB4 = 0; //A
            TRISBbits.TRISB2 = 0; //B
            TRISBbits.TRISB1 = 0; //C
            break;
        case '8':
            TRISBbits.TRISB4 = 0; //A
            TRISBbits.TRISB2 = 0; //B
            TRISBbits.TRISB1 = 0; //C
            TRISAbits.TRISA0 = 0; //D
            TRISAbits.TRISA2 = 0; //E
            TRISBbits.TRISB0 = 0; //F
            TRISBbits.TRISB3 = 0; //G
            break;
        case '9':
            TRISBbits.TRISB4 = 0; //A
            TRISBbits.TRISB2 = 0; //B
            TRISBbits.TRISB1 = 0; //C
            TRISAbits.TRISA0 = 0; //D
            TRISBbits.TRISB0 = 0; //F
            TRISBbits.TRISB3 = 0; //G
            break;
        case '0':
            TRISBbits.TRISB4 = 0; //A
            TRISBbits.TRISB2 = 0; //B
            TRISBbits.TRISB1 = 0; //C
            TRISAbits.TRISA0 = 0; //D
            TRISAbits.TRISA2 = 0; //E
            TRISBbits.TRISB0 = 0; //F
            break;
        case 'a':
            TRISBbits.TRISB4 = 0; //A
            TRISBbits.TRISB2 = 0; //B
            TRISBbits.TRISB1 = 0; //C
            TRISAbits.TRISA0 = 0; //D
            TRISAbits.TRISA2 = 0; //E
            TRISBbits.TRISB0 = 0; //F
            TRISBbits.TRISB3 = 0; //G
            break;
        case 'b':
            TRISBbits.TRISB1 = 0; //C
            TRISAbits.TRISA0 = 0; //D
            TRISAbits.TRISA2 = 0; //E
            TRISBbits.TRISB0 = 0; //F
            TRISBbits.TRISB3 = 0; //G
            break;
        case 'c':
            TRISBbits.TRISB4 = 0; //A
            TRISAbits.TRISA0 = 0; //D
            TRISAbits.TRISA2 = 0; //E
            TRISBbits.TRISB0 = 0; //F
            break;
        case 'd':
            TRISBbits.TRISB2 = 0; //B
            TRISBbits.TRISB1 = 0; //C
            TRISAbits.TRISA0 = 0; //D
            TRISAbits.TRISA2 = 0; //E
            TRISBbits.TRISB3 = 0; //G
            break;
        case 'e':
            TRISBbits.TRISB4 = 0; //A
            TRISAbits.TRISA0 = 0; //D
            TRISAbits.TRISA2 = 0; //E
            TRISBbits.TRISB0 = 0; //F
            TRISBbits.TRISB3 = 0; //G
            break;
        case 'f':
            TRISBbits.TRISB4 = 0; //A
            TRISAbits.TRISA2 = 0; //E
            TRISBbits.TRISB0 = 0; //F
            TRISBbits.TRISB3 = 0; //G
            break;
        case 'g':
            TRISBbits.TRISB4 = 0; //A
            TRISBbits.TRISB1 = 0; //C
            TRISAbits.TRISA0 = 0; //D
            TRISAbits.TRISA2 = 0; //E
            TRISBbits.TRISB0 = 0; //F
            break;
        case 'h':
            TRISBbits.TRISB2 = 0; //B
            TRISBbits.TRISB1 = 0; //C
            TRISAbits.TRISA2 = 0; //E
            TRISBbits.TRISB0 = 0; //F
            TRISBbits.TRISB3 = 0; //G
            break;
        case 'i':
            TRISBbits.TRISB2 = 0; //B
            TRISBbits.TRISB1 = 0; //C
            break;
        case 'j':
            TRISBbits.TRISB2 = 0; //B
            TRISBbits.TRISB1 = 0; //C
            TRISAbits.TRISA0 = 0; //D
            TRISAbits.TRISA2 = 0; //E
            break;
        case 'k':
            TRISBbits.TRISB2 = 0; //B
            TRISAbits.TRISA2 = 0; //E
            TRISBbits.TRISB0 = 0; //F
            TRISBbits.TRISB3 = 0; //G
            break;
        case 'l':
            TRISAbits.TRISA0 = 0; //D
            TRISAbits.TRISA2 = 0; //E
            TRISBbits.TRISB0 = 0; //F
            break;
        case 'm':
            TRISBbits.TRISB4 = 0; //A
            TRISBbits.TRISB1 = 0; //C
            TRISAbits.TRISA2 = 0; //E
            TRISBbits.TRISB3 = 0; //G
            break;
        case 'n':
            TRISBbits.TRISB1 = 0; //C
            TRISAbits.TRISA2 = 0; //E
            TRISBbits.TRISB3 = 0; //G
            break;
        case 'o':
            TRISBbits.TRISB1 = 0; //C
            TRISAbits.TRISA0 = 0; //D
            TRISAbits.TRISA2 = 0; //E
            TRISBbits.TRISB3 = 0; //G
            break;
        case 'p':
            TRISBbits.TRISB4 = 0; //A
            TRISBbits.TRISB2 = 0; //B
            TRISAbits.TRISA2 = 0; //E
            TRISBbits.TRISB0 = 0; //F
            TRISBbits.TRISB3 = 0; //G
            break;
        case 'q':
            TRISBbits.TRISB4 = 0; //A
            TRISBbits.TRISB2 = 0; //B
            TRISBbits.TRISB1 = 0; //C
            TRISBbits.TRISB0 = 0; //F
            TRISBbits.TRISB3 = 0; //G
            break;
        case 'r':
            TRISAbits.TRISA2 = 0; //E
            TRISBbits.TRISB3 = 0; //G
            break;
        case 's':
            TRISBbits.TRISB4 = 0; //A
            TRISBbits.TRISB1 = 0; //C
            TRISAbits.TRISA0 = 0; //D
            TRISBbits.TRISB0 = 0; //F
            TRISBbits.TRISB3 = 0; //G
            break;
        case 't':
            TRISAbits.TRISA0 = 0; //D
            TRISAbits.TRISA2 = 0; //E
            TRISBbits.TRISB0 = 0; //F
            TRISBbits.TRISB3 = 0; //G
            break;
        case 'u':
            TRISBbits.TRISB2 = 0; //B
            TRISBbits.TRISB1 = 0; //C
            TRISAbits.TRISA0 = 0; //D
            TRISAbits.TRISA2 = 0; //E
            TRISBbits.TRISB0 = 0; //F
            break;
        case 'v':
            TRISBbits.TRISB1 = 0; //C
            TRISAbits.TRISA0 = 0; //D
            TRISAbits.TRISA2 = 0; //E
            break;
        case 'w':
            TRISBbits.TRISB4 = 0; //A
            TRISBbits.TRISB1 = 0; //C
            TRISAbits.TRISA0 = 0; //D
            TRISAbits.TRISA2 = 0; //E
            break;
        case 'x':
            TRISBbits.TRISB2 = 0; //B
            TRISBbits.TRISB1 = 0; //C
            TRISAbits.TRISA2 = 0; //E
            TRISBbits.TRISB0 = 0; //F
            TRISBbits.TRISB3 = 0; //G
            break;
        case 'y':
            TRISBbits.TRISB2 = 0; //B
            TRISBbits.TRISB1 = 0; //C
            TRISAbits.TRISA0 = 0; //D
            TRISBbits.TRISB0 = 0; //F
            TRISBbits.TRISB3 = 0; //G
            break;
        case 'z':
            TRISBbits.TRISB4 = 0; //A
            TRISBbits.TRISB2 = 0; //B
            TRISAbits.TRISA0 = 0; //D
            TRISAbits.TRISA2 = 0; //E
            TRISBbits.TRISB3 = 0; //G
            break;

    }
    i++;
    if (i == 4) {
        i = 0;
    }
}

//Takes an int and displays it on the display
void int_to_array(int printNum) {
    breakApartIterate = 0;
    fillIterate = 0;
    if (printNum > 9999) return;
    for (fillIterate = 0; fillIterate < 4; fillIterate++) {
        sevenseg[fillIterate] = 0;
    }
    for (breakApartIterate = 3; breakApartIterate >= 0; breakApartIterate--) {
        sevenseg[breakApartIterate] = (char) (((int) '0')+(printNum % 10));

        printNum = printNum / 10;
    }
}

//Read from I2C
unsigned char I2C_Read_Byte(unsigned char acknowledge_bit) {
    SSP2CON2bits.RCEN = 1; //Set Master as a Receiver
    while (SSP2CON2bits.RCEN == 1); // wait until Reciever has recieved 8 bits
    SSP2CON2bits.ACKDT = acknowledge_bit; //Acknowledge bit should be set to 0 for postive acknowledge
    SSP2CON2bits.ACKEN = 1; //Enable an Acknowledge from the Slave
    while (SSP2CON2bits.ACKEN == 1); // wait until Reciever has recieved 8 bits
    return (SSP2BUF);
}

//Write to I2C
void I2C_Write(unsigned char reg, unsigned char data) {
    SSP2CON2bits.SEN = 1; // Send a start bit
    while (SSP2CON2bits.SEN == 1); //wait for Start to complete
    SSP2BUF = Slave_address; //This sends out the 7 bit  slave address with R/W=0
    while (SSP2STATbits.R_NOT_W == 1); // wait until Ack has been received  (0-> idle, 1-> busy)
    SSP2BUF = reg; //This sends out the 8bits of Data that selects the register inside the part
    while (SSP2STATbits.R_NOT_W == 1); // wait until Ack has been received  (0-> idle, 1-> busy)
    if (SSP2CON2bits.ACKSTAT == 1) {
        while (1);
        I2C_error = -1;
    } //0 = acknowledge  1- no acknowledge
    //SSP2CON2bits.SEN = 1; // Send a start bit
    //while (SSP2CON2bits.SEN == 1); //wait for Start to complete
    //SSP2BUF = Slave_address; //This sends out the 7 bit  slave address with R/W=0
    //while (SSP2STATbits.R_NOT_W == 1); // wait until Ack has been received  (0-> idle, 1-> busy)
    SSP2BUF = data; //This sends out the 8bits of Data that is written to the register addressed above
    while (SSP2STATbits.R_NOT_W == 1); // wait until Ack has been received  (0-> idle, 1-> busy)
    SSP2CON2bits.PEN = 1; // Send a stop bit
    while (SSP2CON2bits.PEN == 1); //wait for Stop to complete  (restart is the same except .RSEN bit)
}

//Read from I2C
unsigned char I2C_Read(unsigned char reg_addr) {


    SSP2CON2bits.SEN = 1;
    while (SSP2CON2bits.SEN == 1);

    SSP2BUF = Slave_address;
    while (SSP2STATbits.R_NOT_W == 1);

    SSP2BUF = reg_addr;
    while (SSP2STATbits.R_NOT_W == 1);

    //this special device needs restart, may not apply others
    SSP2CON2bits.RSEN = 1;
    while (SSP2CON2bits.RSEN == 1);

    SSP2BUF = Slave_address + 1;
    while (SSP2STATbits.R_NOT_W == 1);

    SSP2CON2bits.RCEN = 1;
    while (SSP2CON2bits.RCEN == 1); //wait all 8 bits shifted in


    SSP2CON2bits.PEN = 1;
    while (SSP2CON2bits.PEN == 1);


    return (SSP2BUF);

}

//Takes a frequency in hz and tunes the NS73M via I2C
void set_freq(unsigned long aFrequency) {
    int new_frequency;
    char reg3;
    char reg4;

    //    // New Range Checking... Implement the (experimentally determined) VFO bands:
    //    if (aFrequency < 88500000) { // Band 3
    //        I2C_Write(0x08, 0B00011011);
    //        //Serial.println("Band 3");
    //    } else if (aFrequency < 97900000) { // Band 2
    //        I2C_Write(0x08, 0B00011010);
    //        //Serial.println("Band 2");
    //    } else if (aFrequency < 103000000) { // Band 1
    //        I2C_Write(0x08, 0B00011001);
    //        //Serial.println("Band 1");
    //    } else {
    //        // Must be OVER 103.000.000,                    // Band 0
    //        I2C_Write(0x08, 0B00011000);
    //        //Serial.println("Band 0");
    //    }


    new_frequency = (aFrequency + 304000) / 8192;
    reg3 = new_frequency & 0xff; //extract low byte of frequency register
    reg4 = new_frequency >> 8; //extract high byte of frequency register
    I2C_Write(0x03, reg3); //send low byte
    I2C_Write(0x04, reg4); //send high byte

    //I2C_Write(0x0E, 0B00000101); //software reset

    //Serial.print("Frequency changed to ");
    //Serial.println(aFrequency, DEC);

   //Refresh display
    if (presetMode == 0) {
        int_to_array(userFreq / 100000);
    } else {
        sevenseg[0] = presetName[presetNum][0];
        sevenseg[1] = presetName[presetNum][1];
        sevenseg[2] = presetName[presetNum][2];
        sevenseg[3] = presetName[presetNum][3];
    }

    //I2C_Write(0x00, 0B10100001); //Register 0: 200mV audio input, 75us pre-emphasis on, crystal off, power ON
    //I2C_Write(0x03, 0b00011011); //Set broadcast freq to 97.3

    //I2C_Write(0x04, 0b00101011); //send high byte

    // New Range Checking... Implement the (experimentally determined) VFO bands:
    if (aFrequency < 88500000) { // Band 3
        I2C_Write(0x08, 0b00011011);
        //Serial.println("Band 3");
    } else if (aFrequency < 97900000) { // Band 2
        I2C_Write(0x08, 0b00011010);
        //Serial.println("Band 2");
    } else if (aFrequency < 103000000) { // Band 1
        I2C_Write(0x08, 0b00011001);
        //Serial.println("Band 1");
    } else {
        // Must be OVER 103.000.000,                    // Band 0
        I2C_Write(0x08, 0b00011000);
        //Serial.println("Band 0");
    }

    //I2C_Write(0x08, 0b00011010); //Register 8 defaults This was my original issue

    //I2C_Write(0x00, 0b00000000); //Flip the power switch off

    I2C_Write(0x0E, 0b00000101); //Software reset


    I2C_Write(0x06, 0b00011110); //Set Register 6
}

//Stores data in EEPROM at specified address
void EEPROM_putc(unsigned char address, unsigned char data){
    unsigned char INTCON_SAVE;

    EEADR = address;
    EEDATA = data;

    EECON1bits.EEPGD = 0;   // 0 = Access data EEPROM memory
    EECON1bits.CFGS  = 0;   // 0 = Access Flash program or DATA EEPROM memory
    EECON1bits.WREN  = 1;   // enable write to internal EEPROM

    INTCON_SAVE = INTCON;   //Save INTCON register contents
    INTCON = 0;             // Disable interrupts, Next two lines SHOULD run without interrupts

    EECON2 = 0x55;          //Require sequence for write to internal EEPROM
    EECON2 = 0xaa;          //Require sequence for write to internal EEPROM

    EECON1bits.WR = 1;       //begin write to internal EEPROM

    INTCON = INTCON_SAVE;   //Now we can safely enable interupts if previously used

    while (PIR2bits.EEIF==0); //Wait till write operaiton is complete

    EECON1bits.WREN = 0;    //Disables writes to EEPROM on write complete (EEIF flag on set PIR2)

    PIR2bits.EEIF = 0;      //Clear EEPROM write complete flag
                            //(must be cleared in software. So we do it here)
}

//Read from EEPROM at specified address
unsigned char EEPROM_getc(unsigned char address){
    EEADR = address;
    EECON1bits.EEPGD = 0;   // 0 = Access data EEPROM memory
    EECON1bits.CFGS  = 0;   // 0 = Access Flash program or DATA EEPROM memory
    EECON1bits.RD    = 1;   // EEPROM Read
    return EEDATA;          //return data
}

//Stores an unsigned long into EEPROM from a starting address for 4 consecutive bytes
void EEPROM_putul(unsigned char address, unsigned long data) {
    EEPROM_putc(address + 3, (data & 0x000000FF));
    EEPROM_putc(address + 2, ((data & 0x0000FF00) >> 8));
    EEPROM_putc(address + 1, ((data & 0x00FF0000) >> 16));
    EEPROM_putc(address, ((data & 0xFF000000) >> 24));
}

//Reads an unsigned long from EEPROM at a starting address for 4 consecutive bytes
unsigned long EEPROM_getul(unsigned char address) {
    unsigned long data = 0x0000;
    data = EEPROM_getc(address + 0);
    data = data << 8;
    data = data | EEPROM_getc(address + 1);
    data = data << 8;
    data = data | EEPROM_getc(address + 2);
    data = data << 8;
    data = data | EEPROM_getc(address + 3);

    return data;
}

void main(void) {
    //configure the Oscillator for 4Mhz internal oscillator operation
    OSCCON2 = 0x00; //no 4X PLL
    OSCCON = 0x50; // 4MHZ

    EEPROM_putul(frequencyMem, userFreq);
    userFreq = EEPROM_getul(frequencyMem);

    seven_segment_init();

    TRISD = 0xFF;
    ANSELD = 0x00;

    //Button setup
    TRISEbits.TRISE2 = 1;
    ANSELEbits.ANSE2 = 0;

    Delay1KTCYx(0x11); //Wait a long time to let the I2C transmitter wake up and initilize

    SSP2ADD = 0x1F;
    SSP2CON1 = 0b00101000;

    //Init Tranmitter
    //Load register values with initial default values
    I2C_Write(0x0E, 0b00000101); //Software reset
    I2C_Write(0x01, 0b10110100); //Register 1 defaults
    I2C_Write(0x02, 0b00000101); //Register 2 defaults

    //Check if we are first time boot
    //If so turn power off, and set others to 0
    if(EEPROM_getc(powerMem)==0xFF){
        presetNum = 0;
        presetMode = 0;
        power = 0;

        EEPROM_putc(presetModeMem, presetMode);
        EEPROM_putc(presetNumMem, presetNum);
        EEPROM_putc(powerMem, power);
    }
    else{//Get our stored values
        presetMode = EEPROM_getc(presetModeMem);
        presetNum = EEPROM_getc(presetNumMem);
        power = EEPROM_getc(powerMem);
    }

    // Set initial Power
    if(power == 0){
        I2C_Write(0x00, 0b00000000); //Flip the power switch off
    }
    else{
        I2C_Write(0x00, 0b00000001); //Flip the power switch on
    }

    // Set initial PresetMode
    if(presetMode == 1){
        set_freq(presetFreq[presetNum]);
    }
    else{
        set_freq(userFreq);
    }

    //Check for button presses
    while (1) {
        //Incr btn pressed
        if (PORTBbits.RB5 == 1 && PORTEbits.RE2 == 0) {
            //Check preset mode to see what to do
            if (presetMode == 0) {
                userFreq = userFreq + incrFM;

                //Check if we overflowed the dial
                if(userFreq > topFM) {
                    userFreq = botFM;
                }

                set_freq(userFreq);
            } else { //We are in preset mode
                //Run through presets and display
                presetNum = (presetNum + 1) % (NUM_PRESETS);

                //Store new preset in EEPROM
                EEPROM_putc(presetNumMem, presetNum);

                set_freq(presetFreq[presetNum]);

                sevenseg[0] = presetName[presetNum][0];
                sevenseg[1] = presetName[presetNum][1];
                sevenseg[2] = presetName[presetNum][2];
                sevenseg[3] = presetName[presetNum][3];
            }
        }//Decr btn pressed
        else if (PORTEbits.RE2 == 1 && PORTBbits.RB5 == 0) {
            if(presetMode == 0) {
                userFreq = userFreq - incrFM;
                if (userFreq < botFM) {
                    userFreq = topFM;
                }
                set_freq(userFreq);
            } else {

            }
        }//Both btns pressed
        else if (PORTEbits.RE2 == 1 && PORTBbits.RB5 == 1) {
            while (PORTBbits.RB5 == 1) {
                pressCount++;
                Delay1KTCYx(25);

                //If we hit 30 tell them we are going to do power
                if (pressCount == 30) {
                    sevenseg[0] = ' ';
                    sevenseg[1] = 'p';
                    sevenseg[2] = 'w';
                    sevenseg[3] = 'r';
                } else if (pressCount == 60) { //If we get to 60, presetMode
                    if (presetMode == 0) { //We will tell them the opposite mode of what they are in
                        sevenseg[0] = ' ';
                        sevenseg[1] = 'g';
                        sevenseg[2] = 'e';
                        sevenseg[3] = 'o';
                    } else {
                        sevenseg[0] = 'f';
                        sevenseg[1] = 'r';
                        sevenseg[2] = 'e';
                        sevenseg[3] = 'q';
                    }
                }
            }

            //When we release, if > 60 then change the mode
            if (pressCount > 60) {
                if (presetMode == 1) {
                    //Toggle preset mode
                    presetMode = 0;

                    //Store to EEPROM
                    EEPROM_putc(presetModeMem, presetMode);

                    //They will fall through to the same frequency of the
                    //  last preset they were on

                } else {
                    //Toggle preset mode
                    presetMode = 1;

                    //Store to EEPROM
                    EEPROM_putc(presetModeMem, presetMode);

                    //Go to preset
                    set_freq(presetFreq[presetNum]);
                }
            }//If we were only down for 30, then we are working with power
            else if (pressCount > 30) {
                if (power == 1) {// If on, switch off
                    I2C_Write(0x00, 0b00000000); //Flip the power switch off

                    power = 0;

                    //Store in EEPROM
                    EEPROM_putc(powerMem, power);

                    //Say Off and hold for a sec
                    sevenseg[0] = ' ';
                    sevenseg[1] = 'o';
                    sevenseg[2] = 'f';
                    sevenseg[3] = 'f';
                    Delay10KTCYx(100);
                } else { //If off, switch on
                    I2C_Write(0x00, 0b00000001); //Flip the power switch on

                    power = 1;

                    //Store in EEPROM
                    EEPROM_putc(powerMem, power);

                    //Say on and hold for a sec
                    sevenseg[0] = ' ';
                    sevenseg[1] = ' ';
                    sevenseg[2] = 'o';
                    sevenseg[3] = 'n';
                    Delay10KTCYx(100);
                }
            }

            //Refresh display
            if (presetMode == 0) {
                int_to_array(userFreq / 100000);
            } else {
                sevenseg[0] = presetName[presetNum][0];
                sevenseg[1] = presetName[presetNum][1];
                sevenseg[2] = presetName[presetNum][2];
                sevenseg[3] = presetName[presetNum][3];
            }
        }

        //Reset press count
        pressCount = 0;

        //Delay to adjust freq/preset scroll frequency
        Delay1KTCYx(0xc0);
    }
}